Moveit2 Tutorial 3

# Visualizing In RViz

Run Docker - Open a new terminal and run:

xhost +local:docker && sudo docker run -it --net=host --ipc=host --pid=host --env="DISPLAY=$DISPLAY" --env="QT_X11_NO_MITSHM=1" --env="XAUTHORITY=$XAUTH" --volume="$XAUTH:$XAUTH"   -e LIBGL_ALWAYS_SOFTWARE=1  moveit/moveit2:main-jazzy-tutorial-source

Change the directory to hellow_moveit package

cd ~/ws_moveit/src/hello_moveit/

Open package.xml with nano

sudo nano package.xml

Add this line of code

<depend>moveit_visual_tools</depend>

It will look like this:
Pasted image 20250217092814.png

Press "Ctr + O" to write, press Enter to confirm , Press "Ctr +X" to exit it.

Open CMakeLists.txtl with nano

sudo nano CMakeLists.txt

Add this line to the section of find_package statements:

find_package(moveit_visual_tools REQUIRED)

It will look like this:
Pasted image 20250217093333.png

Add this line to the session of "the ament_target_dependencies"

"moveit_visual_tools"

It will look like this:
Pasted image 20250217093657.png

Press "Ctr + O" to write, press Enter to confirm , Press "Ctr +X" to exit it.

Open hello_moveit.cpp with nano

sudo nano ~/ws_moveit/src/hello_moveit/src/hello_moveit.cpp

add this line:

#include <moveit_visual_tools/moveit_visual_tools.h>

It will look like this:
Pasted image 20250217094250.png
Press "Ctr + O" to write, press Enter to confirm , Press "Ctr +X" to exit it.

colcon build the ROS package

cd ~/ws_moveit/ && colcon build --packages-select hello_moveit

source the newly build ROS package

cd ~/ws_moveit/ && source install/setup.bash

Save the state of the docker.

*Open a new tab of terminal, clicking this:

You will see new tab will be opened*
Pasted image 20250213162851.png

Use the new tab, find the current running docker ID

docker ps

Pasted image 20250213163031.png

**Copy your docker ID here!

save and commit the docker (Replace with your docker ID)

sudo docker commit {docker_id}  moveit/moveit2:main-jazzy-tutorial-source

Pasted image 20250213163237.png

This will save your work. If you are not doing it properly, you will lose your work!

***Now return to your "previous tab" running Docker, and exit it.

exit

Close all the terminals.

More coding

Run Docker - Open a new terminal and run:

xhost +local:docker && sudo docker run -it --net=host --ipc=host --pid=host --env="DISPLAY=$DISPLAY" --env="QT_X11_NO_MITSHM=1" --env="XAUTHORITY=$XAUTH" --volume="$XAUTH:$XAUTH"   -e LIBGL_ALWAYS_SOFTWARE=1  moveit/moveit2:main-jazzy-tutorial-source

Change the directory to hellow_moveit package

cd ~/ws_moveit/src/hello_moveit/

Open hello_moveit.cpp with nano

sudo nano ~/ws_moveit/src/hello_moveit/src/hello_moveit.cpp

Add this line at the top:

#include <thread>  // <---- add this to the set of includes at the top

It will look like this:
Pasted image 20250217100652.png

Add this line in the Main function:

  // We spin up a SingleThreadedExecutor for the current state monitor to get
  // information about the robot's state.
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

It will look like this:
Pasted image 20250217101239.png

Add this line at the end:

spinner.join();  // <--- Join the thread before exiting

It will look like this:
Pasted image 20250217101608.png

Press "Ctr + O" to write, press Enter to confirm , Press "Ctr +X" to exit it.

Open hello_moveit.cpp with nano

sudo nano ~/ws_moveit/src/hello_moveit/src/hello_moveit.cpp

Create and Initialize MoveItVisualTools

Add this code:

  // Construct and initialize MoveItVisualTools
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              move_group_interface.getRobotModel() };
  moveit_visual_tools.deleteAllMarkers();
  moveit_visual_tools.loadRemoteControl();

It will look like this:
Pasted image 20250217221005.png

Add this code:

  // Create a closure for updating the text in rviz
  auto const draw_title = [&moveit_visual_tools](auto text) {
    auto const text_pose = [] {
      auto msg = Eigen::Isometry3d::Identity();
      msg.translation().z() = 1.0;  // Place text 1m above the base link
      return msg;
    }();
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
  };
  auto const prompt = [&moveit_visual_tools](auto text) { moveit_visual_tools.prompt(text); };
  auto const draw_trajectory_tool_path =
      [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("manipulator")](
          auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };

It will look like this:
Pasted image 20250217221120.png

Add this code:

// Create a plan to that target pose
prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");
moveit_visual_tools.trigger();

It will look like this:
Pasted image 20250217103349.png

Modify the code below (Replace it by copy and paste):

  // Execute the plan
  if (success)
  {
    draw_trajectory_tool_path(plan.trajectory);
    moveit_visual_tools.trigger();
    prompt("Press 'next' in the RvizVisualToolsGui window to execute");
    draw_title("Executing");
    moveit_visual_tools.trigger();
    move_group_interface.execute(plan);
  }
  else
  {
    draw_title("Planning Failed!");
    moveit_visual_tools.trigger();
    RCLCPP_ERROR(logger, "Planning failed!");
  }

it will look like this:
Pasted image 20250217224157.png

Press "Ctr + O" to write, press Enter to confirm , Press "Ctr +X" to exit it.

colcon build the ROS package

cd ~/ws_moveit/ && colcon build --packages-select hello_moveit

source the newly build ROS package

cd ~/ws_moveit/ && source install/setup.bash

{Optional} If you have lost track of the code and encounter a compilation error, you can replace the entire code by copying and pasting this.

hello_moveit.cpp (tutorial 3)

and colcon build the ROS package again

cd ~/ws_moveit/ && colcon build --packages-select hello_moveit

source the newly build ROS package

cd ~/ws_moveit/ && source install/setup.bash

Save the state of the docker.

*Open a new tab of terminal, clicking this:

You will see new tab will be opened*
Pasted image 20250213162851.png

Use the new tab, find the current running docker ID

docker ps

Pasted image 20250213163031.png

**Copy your docker ID here!

save and commit the docker (Replace with your docker ID)

sudo docker commit {docker_id}  moveit/moveit2:main-jazzy-tutorial-source

Pasted image 20250213163237.png

This will save your work. If you are not doing it properly, you will lose your work!

***Now return to your "previous tab" running Docker, and exit it.

exit

Close all the terminals.

Enable visualizations in RViz

Run Docker - Open a new terminal and run:

xhost +local:docker && sudo docker run -it --net=host --ipc=host --pid=host --env="DISPLAY=$DISPLAY" --env="QT_X11_NO_MITSHM=1" --env="XAUTHORITY=$XAUTH" --volume="$XAUTH:$XAUTH"   -e LIBGL_ALWAYS_SOFTWARE=1  moveit/moveit2:main-jazzy-tutorial-source

Run the RViz

ros2 launch moveit2_tutorials demo.launch.py

Uncheck “MotionPlanning” in the “Displays” tab to hide it. We aren’t going to be using the “MotionPlanning” plugin for this next part.

Pasted image 20250217150521.png

To add the buttons to interact with the prompts we added to our program open the dialog with the “Panels/Add New Panel” menu:

Pasted image 20250217150724.png

Pasted image 20250217150808.png

Finally, we need to add a Marker Array to render the visualizations we’ve added. Click on the “Add” Button in the “Displays” panel.

Pasted image 20250217150945.png

Pasted image 20250217151021.png

Scroll to the bottom of the items in the Displays panel and edit the topic that the new Marker Array is using to /visualization marker array.
Pasted image 20250217151114.png

You are now ready to run your new program with visualizations.

In a new terminal, go to the workspace, source the workspace, and run hello_moveit:

xhost +local:docker && sudo docker run -it --net=host --ipc=host --pid=host --env="DISPLAY=$DISPLAY" --env="QT_X11_NO_MITSHM=1" --env="XAUTHORITY=$XAUTH" --volume="$XAUTH:$XAUTH"   -e LIBGL_ALWAYS_SOFTWARE=1  moveit/moveit2:main-jazzy-tutorial-source

ros2 run hello_moveit hello_moveit

You’ll notice that your program has stopped with a log that looks like this:
Pasted image 20250217151408.png

Click the Next button in RViz and see your application advance.
Pasted image 20250217151554.png

Pasted image 20250217151620.png

Go to Tutorial 4Moveit2 Tutorial 4

Reference : MoveIt Quickstart in RViz — MoveIt Documentation: Rolling documentation